//=====================================================================================================
// MahonyAHRS.cpp
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author			Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MahonyAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions
#define MahonyAHRS_sampleFreq	(61.0f) // sample frequency in Hz
#define twoKpDef				(2.0f * 0.7f)	// 2 * proportional gain
#define twoKiDef				(2.0f * 0.1f)	// 2 * integral gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
// Function declarations

float MahonyAHRSinvSqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
    float qwqw, qwqx, qwqy, qwqz, qxqx, qxqy, qxqz, qyqy, qyqz, qzqz;  
	float hx, hy, bx, bz;
	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
		MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
		return;
	}

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = MahonyAHRSinvSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;     

		// Normalise magnetometer measurement
		recipNorm = MahonyAHRSinvSqrt(mx * mx + my * my + mz * mz);
		mx *= recipNorm;
		my *= recipNorm;
		mz *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        qwqw = qw * qw;
        qwqx = qw * qx;
        qwqy = qw * qy;
        qwqz = qw * qz;
        qxqx = qx * qx;
        qxqy = qx * qy;
        qxqz = qx * qz;
        qyqy = qy * qy;
        qyqz = qy * qz;
        qzqz = qz * qz;   

        // Reference direction of Earth's magnetic field
        hx = 2.0f * (mx * (0.5f - qyqy - qzqz) + my * (qxqy - qwqz) + mz * (qxqz + qwqy));
        hy = 2.0f * (mx * (qxqy + qwqz) + my * (0.5f - qxqx - qzqz) + mz * (qyqz - qwqx));
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (qxqz - qwqy) + my * (qyqz + qwqx) + mz * (0.5f - qxqx - qyqy));

		// Estimated direction of gravity and magnetic field
		halfvx = qxqz - qwqy;
		halfvy = qwqx + qyqz;
		halfvz = qwqw - 0.5f + qzqz;
        halfwx = bx * (0.5f - qyqy - qzqz) + bz * (qxqz - qwqy);
        halfwy = bx * (qxqy - qwqz) + bz * (qwqx + qyqz);
        halfwz = bx * (qwqy + qxqz) + bz * (0.5f - qxqx - qyqy);  
	
		// Error is sum of cross product between estimated direction and measured direction of field vectors
		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / MahonyAHRS_sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / MahonyAHRS_sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / MahonyAHRS_sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));
	gz *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));
	qa = qw;
	qb = qx;
	qc = qy;
	qw += (-qb * gx - qc * gy - qz * gz);
	qx += (qa * gx + qc * gz - qz * gy);
	qy += (qa * gy - qb * gz + qz * gx);
	qz += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = MahonyAHRSinvSqrt(qw * qw + qx * qx + qy * qy + qz * qz);
	qw *= recipNorm;
	qx *= recipNorm;
	qy *= recipNorm;
	qz *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = MahonyAHRSinvSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;        

		// Estimated direction of gravity and vector perpendicular to magnetic flux
		halfvx = qx * qz - qw * qy;
		halfvy = qw * qx + qy * qz;
		halfvz = qw * qw - 0.5f + qz * qz;
	
		// Error is sum of cross product between estimated and measured direction of gravity
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / MahonyAHRS_sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / MahonyAHRS_sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / MahonyAHRS_sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));
	gz *= (0.5f * (1.0f / MahonyAHRS_sampleFreq));
	qa = qw;
	qb = qx;
	qc = qy;
	qw += (-qb * gx - qc * gy - qz * gz);
	qx += (qa * gx + qc * gz - qz * gy);
	qy += (qa * gy - qb * gz + qz * gx);
	qz += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = MahonyAHRSinvSqrt(qw * qw + qx * qx + qy * qy + qz * qz);
	qw *= recipNorm;
	qx *= recipNorm;
	qy *= recipNorm;
	qz *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root

float MahonyAHRSinvSqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//====================================================================================================
// END OF CODE
//====================================================================================================
